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@ Control device for servo motor. 



@ A control device for a servo motor comprises 
feedforward means for correcting a speed reference 
obtained by a position control device by a correction 
amount obtained on tlie basis of an objective posi- 
tion or an objective speed of the servo motor so that 
a response delay of the position control device for 
obtaining a reference speed necessary to control a 
rotation angle position of the servo motor to an 
objective rotation angle position is compensated for 
while restricting vibration. 
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CONTROL DEVICE FOR SERVO MOTOR 



BACKGROUND OF THE INVENTION 



Field of the invention 

The present invention relates to a control de- 
vice for servo motors used to drive industrial ro- 
bots, etc. 



Description of the Prior Art 

In order to move a top portion of an industrial 
robot such as an arm thereof, along a predeter- 
mined passage at a desired speed by a servo 
motor, it has been usual to control the servo motor 
on the basis of objective values of a position or 
angle to which the arm is to be settled, and a 
moving speed of the arm. which is obtained by 
calculations. In a conventional control device for 
driving such industrial robot, a positional deviation 
AP IS obtained by integrating a difference Ao) = o>* 
- Ci>tn between a measured value of rotational 
speed of the servo motor for driving a mechanical 
system composed of the anm. etc.. and an objec- 
tive rotational speed w* thereof and a reference 
value ojp* of rotational speed of the servo motor at 
which the positional deviation AP becomes zero, is 
obtained in a position control device by multiplying 
a signal indicative of the positional deviation AP 
with a predetermined gain Kj. Then, a reference 
torque value T* of the servo motor at which a 
difference between the reference speed value «p* 
and the measured speed value um of the servo 
motor becomes zero, is obtained by a speed con- 
trol device. An armature current i of the servo 
motor is regulated by a power converter on the 
basis of the reference torque T* to drive the me- 
chanical system of the robot. Gains of respective 
control elements are regulated such that a dynamic 
characteristics of the mechanical system becomes 
optimum. 

In the case of a robot having multiple articu- 
lates, the moment of Inertia of the arm around the 
root portion thereof is large due to the cantilever 
type of support structure, and the natural frequency 
of vibration becomes smaller, to facilitate vibration 
of the mechanical system comprising the arm, etc. 
In order to restrict such vibration, it has been usual 
to make the gain Kj of the position control device 
smaller to thereby reduce the control response or 
to make time dependency of variation of the objec- 
tive input ctf" smaller. 



In the former solution, not only does the opera- 
tion time of the mechanical system becomes long- 
er, but also the positional deviation during opera- 
tion increases, resulting in the accuracy of continu- 
5 ous interpolation for moving passage being lower- 
ed. 



SUMMARY OF THE INVENTION 

10 

An object of the present invention is to provide 
a control device for a servo motor by which vibra- 
tion of a mechanical system to be driven by the 

75 servo motor is restricted while making the delay of 
the control response smaller. 

The above object can be achieved according to 
the present invention by the provision of a servo 
motor control device including position control 

20 means for obtaining a speed reference for control- 
ling the angular position of the servo motor to an 
objective angular position, speed control means for 
obtaining a reference torque at which a speed 
deviation between a measured rotational speed of 

25 the servo motor and a reference speed obtained by 
the position control means becomes zero and a 
power converter for controlling an armature current 
of the servo motor on the basis of a reference 
torque obtained by the speed control means, char- 

30 acterized by comprising feedforward means for ob- 
taining the correction amount necessary to com- 
pensate a response delay of the position control 
means and means for correcting the reference 
speed obtained by the position control means with 

35 a correction amount obtained by the feedforward 
means. 



BRIEF DESCRIPTION OF THE DRAWING 

40 

Fig. 1 is a block circuit diagram showing a 
first embodiment of a control device for a servo 
motor, according to the present invention; 
45 Fig. 2 is an explanatory block diagram for an 

operation of a feedforward element of the control 
device shown in Fig. 1; 

Fig. 3 is a graph showing a step response of 
the feedforward element of the control device 
50 shown in Fig. 1; 

Fig. 4 is a block circuit diagram of a second 
embodiment of the present control device; 

Fig. 5 is a block circuit diagram of a third 
embodiment of the present control device; 
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Fig. 6 is an explanatory block diagram for an 
operation of a feedforward element of the device 
shown in Rg. 5; and 

Fig. 7 is a block circuit diagram of a fourth 
embodiment of the present control device. 



DETAILED DESCRIPTION OF THE PREFERRED 
EMBODIMENTS 



A first embodiment of the present control de- 
vice is shown in Fig. 1 . The control device accord- 
ing to this embodiment includes an adder 1, a 
position control device 2. a feedforward element 6, 
adders 8 and 9, a speed control device 10. a 
damper element 11 and a power converter 13. The 
adder 1 calculates a deviation A<d between an 
objective rotational speed value ta and a measured 
speed value of a servo motor 14 for driving a 
mechanical system 15 including an arm of a robot, 
etc. In the position control device 2, the deviation 
Aat obtained by the adder 1 is integrated to obtain 
a position deviation which is multiplied with a suit- 
able gain Ki to obtain a rotational speed reference 
value o)i of the servo motor 14 at which the position 
deviation becomes zero. 

In order to compensate for response delay of 
the position control device 2, a correction amount 
wf for the reference value wj of rotational speed of 
the servo motor 14 is obtained, at the same time, 
by the feedforward element 6 on the basis of the 
objective rotational speed value w". The transfer 
function Gr(S) of the feedforward element 6 is re- 
presented by the following equation (1). 
G,(S) = (1 -K, + T,'S)/(1 + T,'S) (1) 
where Kf is a coefficient satisfying 0 ^ Kf ^ 1 and 
T( is a positive coefficient 

A summation of the reference value <oi of rota- 
tional speed obtained by the position control device 
2 and the correction amount <jf from the feedfor- 
ward element 6 is performed in the adder 8, the 
result being used as a corrected reference rota- 
tional speed value wp". A deviation between the 
corrected reference value «f" and the measured 
rotational speed value <om of the servo motor 14 is 
obtained by the adder 9. On the basis of the 
deviation, a torque instruction Tp for the servo 
motor 14 at which the deviation becomes zero is 
obtained by the speed control device 10. 

On the other hand, the speed and acceleration 
of the robot arm of the mechanical system 15 are 
estimated by the damper element 11 on the basis 
of the corrected reference rotational speed value 
«p* and the measured rotational speed of the 
servo motor 14 and, then, on the basis of the 
estimations, a torque instruction T<j for improving 
the damping characteristics of the robot arm is 



obtained by the damper element 11 . A summation 
of the torque instruction Tj and an output Tp of the 
speed control device 10 is performed by the adder 
12, the output of which becomes a reference 

5 torque T of the servo motor 14. On the basis of T*. 
an armature current I of the servo motor 14 is 
controlled by the power converter 13, so that the 
speed of the robot arm of the mechanical sys- 
tem 15 is regulated to a desired value. 

10 In such control device as above, the com- 

pensation for response delay of control of the posi- 
tion control device 2 is performed as shown in Fig. 
2. In Fig. 2, a reference numeral 19 shows a 
control loop from the adder 9 in Fig. 1 to the servo 

T5 motor 14, which has a transfer function Q. A sym- 
bol Gf shows a transfer function of the feedforward 
element 6. A transfer function np7Q" from the 
aimed rotation speed to a corrected reference 
rotation speed value up* can be represented as 

20 follows: 

QpVQ- = (G, + Kj/S)/(1 + Kj- Q/S) (2) 
where it is assumed that Q can be approximated to 
the following primary delay factor. 
Q = 1/(1 + Tq'S) (3) 

25 By inserting the equations (1) and (3) into the 

equation (2). the following is obtained: 
Qp/n" = (Ki + (1-Kf + Ki-T,)S + T,' S2)(1+Tq-S)/[- 
(Ki + S + TqS2)(1+T,-S)] (4) 
Assuming Tt = Tq and Kf = Kj* Tu the equation 

30 (4) can be written as: 
JIpVO- = 1 (5) 

Therefore, by suitably selecting the feedforward 
element 6, the control response delay of the posi- 
tion control device 2 can be compensated for. 

35 A step response of the feedforward element 6 

selected as above is shown in Fig. 3. From this 
step response, it is clear that a compensation of 
transient deviation of the primary delay system 
having an arbitrary time constant and regulation of 

40 the normal feedforward amount are possible by 
suitably changing the coefficients Tf and Kf of the 
transfer function Qf of the feedforward element 6 
(cf. the equation (1)). For example, when Kf = 1, 
only the transient deviation of the primary delay 

45 system can be compensated for. 

According to this embodiment in which the 
response delay of the position control element 2 
can be compensated for by means of the feedfor- 
ward element 6, it Is possible to reduce the post* 

50 tional deviation and increase the control response 
speed. Further, with the feedforward element ca- 
pable of not only compensating transient deviation 
of the primary delay system having an arbitrary 
time constant but also of arbitrarily setting the 

55 normal feedforward amount, it is possible to pre- 
vent overshoot due to excessive feedforward. In 
addition, It is possible to restrict undesired vibration 
by estimating the speed and acceleration of the 
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robot arm to be driven by the servo motor and 
feedback-controllmg it on the basis of the estima- 
tions. 

Although, in the first embodiment, the feedback 
control is performed by the damper element the 
damper element can be substituted by any other 
means. Or, when the damping characteristics of the 
mechanical system itself are acceptable, such 
damper element can be omitted. 

Fig. 4 shows a second embodiment of the 
present invention. A control device according to the 
second embodiment includes an integrator 3, an 
adder 4, a position control device 5. a feedforward 
element 7. adders 8 and 9, a speed control device 
10. a damper element 11, an adder 12 and a power 
converter 13. Since all components of the second 
embodiment except the integrator 3. the adder 4, 
the position control device 5 and the feedforward 
element 7 are the same as those shown in Fig. 1 , 
details thereof are omitted for avoidance of duplica- 
tion. The measured rotational speed value of 
the servo motor 14 is integrated in the integrator 3 
to obtain a rotation angle dm of the servo motor 14, 
which is subtracted from the objective rotation an- 
gle 6' thereof in the adder 4 to obtain a positional 
deviation AO therebetween. The positional deviation 
A9 thus obtained is multiplied with the gain Kj by 
the position control device 5 and a reference rota- 
tional speed value <oi of the servo motor 14 at 
which the deviation AO becomes zero Is obtained. 

On the other hand, in order to compensate for 
the response delay of the position control device 5 
on the basis of the objective rotation angle value d*. 
a correction value «i for the speed of the servo 
motor 14 with respect to the reference value is 
obtained by the feedforward element 7. The trans- 
fer function Gf(S) of the feedforward element 7 in 
this case becomes as follows: 
Gf(S) = [S(1.K, + T,*S)]/(1 +Tf-S) (6) 
where K, is a coefficient satisfying 0 :S K| S 1 and 
T| is a positive coefficient. 

A sum of the reference value from the posi- 
tion control device 5 and the correction value oif 
from the feedforward element 7 is obtained in the 
adder 8 as a corrected reference rotational speed 
value <op*. Then, as in the first embodiment, the 
servo motor 14 is controlled through the speed 
control device 10. the damper element 11 and the 
power converter 13. on the basis of the corrected 
reference value wp" and the measured speed value 
«m. The response delay of the position control 
device 5 is also compensated for by the feedfor- 
ward elenr>ent 7 as in the same manner as de- 
scribed with reference to the first embodiment. 

According to the second embodiment, it is 
possible to reduce the positional deviation and 
speed-up the control response due to the fact that 
the response delay of the position control device 5 



is compensated by the feedforward element 7. 
Further, since the feedforward element 7 compen- 
sates for a transient deviation of the primary delay 
system having an arbitrary time constant and 
5 makes the normal feedfon^/ard amount possible to 
set arbitrarily, it Is possible to prevent overshoot 
due to an excess amount of feedforward from oc- 
curring. In addition, since the feedback control is 
performed by using estimated speed and accelera- 

10 tion of the robot arm. It is possible to prevent 
vibration from occurring. 

Fig. 5 shows a third embodiment of the control 
device for a servo motor. The embodiment in- 
cludes outside the position control loop, a correc- 

15 tion loop for correcting a position with respect to 
the objective position, which has means Including 
an adder 21 and a coordinate transformer 22. for 
totally controlling an operation of a whole robot, 
and means including an adder 4. a position control 

20 device 5, a feedforward element 7, an adder 8. a 
speed control device 5 and a power converter 13, 
for controlling servo motors 14 of respective articu- 
lates of the robot, although Fig. 5 shows a con- 
struction for one articulate. 

25 A difference Per between a position Pr of a tool 

of the robot and a desired work point Pj with 
respect to a work is detected by a distance sensor 
20 mounted on a top end of the robot and a 
deviation between the difference Pgr and an aimed 

00 position Po" obtained from a predetermined instruc- 
tion data is obtained by an adder 21 as a corrected 
objective position P\ The corrected objective posi- 
tion P" is transformed into an objective rotation 
angle of an i-th servo motor of the servo motors 14 

35 of the respective drive shafts of the robot and 9' is 
obtained as an objective rotation angle of the servo 
motor 1 4 of an i-th drive shaft. 

The measured rotational speed of the servo 
motor 14 is integrated by the integrator 3 to obtain 

40 the measured rotation angle 9^ which is processed 
by the adder 4 to obtain a deviation (fl" - 9„) of It 
with respect to the objective value A resulting 
positional deviation is multiplied with a suitable 
gain Kj by the position control device 5 so that a 

45 reference rotational speed of the servo motor 14 
at which the above positional deviation becomes 
zero is obtained. 

On the basis of the objective rotation angle 9*. 
a correction amount wf for the reference rotational 

50 speed an of the servo motor 14 is obtained by the 
feedforward element 7 upon which the compensa- 
tion of the response delay of the position control 
device 5 is realized. The transfer function Gf(S) of 
the feedforward element 7 for this case is repre- 
ss sented by the aforementioned equation (6). 

A sum of the reference rotational speed <ai from 
the position control device 5 and the correction 
amount wf for the reference value <au which is 
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provided by the feedforward element 7 is obtained 
by the adder 8 as a corrected reference rotational 
speed ojp*. A difference (wp - wm) between the 
con-ected reference value «p* and the measured 
rotational speed aim of the servo nnotor 14 is ob- 
tained by the adder 9 and a torque instruction T of 
the servo motor 14 at which the difference be- 
comes zero is obtained by the speed control de- 
vice 10. On the basis of this torque instruction T, 
an armature current of the servo motor 14 is con- 
trolled by the power converter 13 to drive the 
mechanical system 1 5 of the robot. 

Fig. 6 shows how to compensate for the re- 
sponse delay of the position control device 5 in the 
construction shown in Fig. 5. In Fig. 6, a reference 
numeral 19 depicts the whole control elements 
effective in obtaining the rotational speed of the 
servo motor 14 from the output of the adder 8 in 
Fig, 5 and the control object which has a transfer 
function Q. The transfer function of the feedforward 
element 7 is also depicted by Gf. The transfer 
function QVe' from the reference rotation angle 6* to 
the corrected reference rotational speed cop* can be 
represented as follows: 
n-'0- = (Ki + Q,)/(1 + Ki*Q/S) (7) 
where it is assumed that Q can be approximated 
by the primary delay element such as shown by 
the equation (3). 

By using the equations (1 ) and (3) in the equa- 
tion (7). it can be rewritten as follow: 
nVO" =: [K| + (1.Kt + K;*T,)S + T,'S2]{1+Tq-S)- 
•S/[(Ki + S + Tq-S2)(1 + T,-S)] (8) 
Assuming Tf = Tq and Kf = Kj'Tt. the equation (8) 
can be rewritten as follows: 
n-'0- = s (9) 

Therefore, it is possible to compensate the re- 
sponse delay of the position control device 5 by 
suitably selecting the feedforward element 7. 

According to this embodiment by which the 
response delay of the control device 5 can be 
compensated for by the feedforward element 7 and 
in which the feedfonfl^ard element allows a arbitrary 
setting of the normal feedforward amount, it is 
possible to reduce the positional deviation of the 
robot etc. to thereby shorten the control response 
for contour turning, to compensate for transient 
deviation of the primary delay system having an 
arbitrary time constant and to prevent overshoot 
due to excess feedforward. 

Fig. 7 shows a fourth embodiment of the 
present invention. A control device according to 
this embodiment corresponds substantially to the 
third embodiment shown in Fig. 5 having no coordi- 
nate transformer and is used in cases where a 
moving direction of a machine to be driven by a 
servo motor coincides with a detecting direction of 
a distance sensor. 

A distance sensor 23 mounted on a top end of 



a robot detects a distance between a position Pr of 
a tool of the robot and a desired work point Pj as a 
deviation which is converted into an angle deviation 
fler corresponding to a rotation angle of the servo 
5 motor 14. A difference between the angle deviation 
6er and an aimed rotation angle fio* of the servo 
motor obtained by an instruction is obtained by an 
adder 24 to obtain a corrected aimed rotation angle 
d". Constructive components other than the dis- 
10 tance sensor 23 and the adder 24 function similarly 
to the third embodiment shown in Fig. 5 and per- 
form a contour turning control. 

Also, in this embodiment by which the re- 
sponse delay of the control device 5 can be com- 
ts pensated for by the feedforward element 7 and in 
which an output of the distance sensor 23 is input 
to the servo system without coordinate transforma- 
tion, it is possible to reduce positional deviation to 
thereby quicker control response for contour turn- 
so ing, to simplify the construction of the control de- 
vice and to reduce time delays due to the coordi- 
nate transformation. 



25 Claims 

1 . A control device of a servo motor including 
position control means for obtaining a reference 
speed necessary to control a rotational angle posi* 

30 tion of the servo motor to an objective rotational 
angle position, speed control means for obtaining a 
reference torque necessary to make a speed de* 
viation between a rotational speed of the servo 
motor and the reference speed obtained by said 

35 position control means zero and a power converter 
for controlling an armature current of the servo 
motor on the basis of the reference torque obtained 
by said speed control means, comprising feedfor- 
ward means (6) for obtaining the amount of correc- 

40 tion necessary to compensate for response delay 
of said position control means and means (8) for 
correcting the speed reference obtained by said 
position control means by the correction amount 
obtained by said feedforward means. 

45 2- The control device as claimed in claim 1. 

wherein said position control means (2) includes an 
integrating component to obtain said reference 
speed on the basis of a speed deviation between 
said rotational speed and an objective speed of the 

50 servo motor and wherein said feedforward means 
(6) functions to obtain said correction amount on 
the basis of said objective speed and has a transfer 
function Gf(S) represented by the following equa- 
tion 

55 G|(S) » (1 - Kf + Tf'Sy(1 + T,*S) 

wherein Kf is a coefficient satisfying 0 ^ Kf ^ 1 and 
Tf is a positive coefficient. 
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3. The control device as claimed rn claim 2, 
further comprising damper means (11) for obtaining 
a torque instruction necessary to improve a damp- 
ing characteristics of a mechanical system as a 
load of said servo motor on the basis of said 5 
corrected reference speed and said rotational 
speed of said servo motor and correcting said 
reference torque by said torque instruction. 

4. The control device as claimed in claim 1. 
further comprising integration means (3) for obtain- io 
ing a rotation angle of said servo motor on the 
basis of said rotation speed of said servo motor 

and wherein said position control means (2) pro- 
vides a speed reference at which an angle de- 
viation between said objective rotation angle and 75 
said rotation angle obtained by said integration 
means becomes zero and wherein said feedforward 
means functions to obtain said correction amount 
on the basis of said objective rotation angle and 
has the following transfer function Qf{S) 20 
Qf(S) = S(1 • Kf + Tf-S)/(1 + Tf*S) 
wherein Kf Is a coefficient satisfying 0 ^ Kf S ^ and 
T| is a positive coefficient 

5. The control device as claimed 4. further 
comprising damper means (11) for obtaining a 25 
torque instruction necessary to Improve a damping 
characteristics of a mechanical system as a load of 
said servo motor on the basis of said corrected 
reference speed and said rotation speed of said 
servo motor and correcting said torque reference 30 
by the torque instruction. 

6. The control device as claimed in claim 4, 
further comprising position deviation detection 
means (20) for detecting a positional deviation be- 
tween a position of said robot and a desired posi- as 
tion thereof and objective position correction 
means (20, 21, 22) for correcting said objective 
rotation angle of said servo motor on tiie basis of 
said positional deviation detected by said position 
deviation detection means. 40 

7. The control device as claimed in claim 6, 
wherein said objective position correction means 
includes means (20. 21) for correcting said objec- 
tive position of said robot by said positional de- 
viation detected by said position deviation detec- 45 
tion means and coordinate conversion means (22) 

for converting said corrected objective position Into 
an objective rotation angle. 

8. The control device as claimed in claim 4, 
further comprising position deviation detection so 
means (29) for detecting a rotation angle deviation 

on the basis of a positional deviation between a 
position of said robot and a desired position thereof 
and objective position correction means (24) for 
correcting said objective position of said robot by a 55 
rotation angle deviation detected by said position 
deviation detection means. 
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@ Control device for servo motor. 



© A control device for a servo motor comprises 
feedforward means for correcting a speed reference 
obtained by a position control device by a correction 
amount obtained on the basis of an objective posi- 
tion or an objective speed of the servo motor so that 



a response delay of the position control device for 
obtaining a reference speed necessary to control a 
rotation angle position of the servo motor to an 
objective rotation angle position is compensated for 
while restricting vibration. 
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